#include "manifest.h"

typedef struct {
    pthread_mutex_t mx;
    int fd;
    void* sm;
} ashmem_mutex_t;

pthread_mutex_t* ashmem_mutex_create(const char *name) {
    /* sizeof(ashmem_mutex_t) == 12 */
    assertion(sizeof(ashmem_mutex_t) <= PAGE_SIZE, "sizeof(ashmem_mutex_t)=%d PAGE_SIZE=%d", sizeof(ashmem_mutex_t), PAGE_SIZE);
    pthread_mutex_t* m = null;
    int fd = ashmem_create_region(name, PAGE_SIZE);
    if (fd < 0) {
        return null;
    }
    int r = 0;
    r = ashmem_set_prot_region(fd, PROT_READ | PROT_WRITE);
    if (r != 0) {
        close(fd);
        return null;
    }
    void* sm = mmap(null, PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
    if (sm != null) {
        pthread_mutexattr_t ma;
        pthread_mutexattr_init(&ma);
        r = pthread_mutexattr_setpshared(&ma, PTHREAD_PROCESS_SHARED);
        if (r == 0) {
            m = (pthread_mutex_t*)sm;
            r = pthread_mutex_init(m, &ma);
        }
    }
    if (sm == null || r != 0) {
        if (sm != null) {
            munmap(sm, PAGE_SIZE);
        }
        close(fd);
        return null;
    }
    ashmem_mutex_t* zmx = (ashmem_mutex_t*)m;
    zmx->fd = fd;
    zmx->sm = sm;
    return m;
}

void ashmem_mutex_destroy(pthread_mutex_t* m) {
    ashmem_mutex_t* zmx = (ashmem_mutex_t*)m;
    int fd = zmx->fd;
    void* sm = zmx->sm;
    int r = pthread_mutex_destroy(m);
    assertion(r == 0, "pthread_mutex_destroy=%d", r);
    r = munmap(sm, PAGE_SIZE);
    assertion(r == 0, "munmap(0x%08X)=%d", sm, r);
    r = close(fd);
    assertion(r == 0, "close(fd=%d)=%d", fd, r);
}

typedef struct {
    sem_t sem;
    int fd;
    void* sm;
} ashmem_sem_t;

sem_t* ashmem_sem_create(const char *name) {
    /* sizeof(ashmem_sem_t) == 12 */
    assertion(sizeof(ashmem_sem_t) <= PAGE_SIZE, "sizeof(ashmem_sem_t)=%d PAGE_SIZE=%d", sizeof(ashmem_sem_t), PAGE_SIZE);
    sem_t* s = null;
    int fd = ashmem_create_region(name, PAGE_SIZE);
    if (fd < 0) {
        return null;
    }
    int r = ashmem_set_prot_region(fd, PROT_READ | PROT_WRITE);
    if (r != 0) {
        close(fd);
        return null;
    }
    void* sm = mmap(null, PAGE_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
    if (sm != null) {
        s = (sem_t*)sm;
        r = sem_init(s, 1, 0);
    }
    if (sm == null || r != 0) {
        if (sm != null) {
            munmap(sm, PAGE_SIZE);
        }
        close(fd);
        return null;
    }
    ashmem_sem_t* zsm = (ashmem_sem_t*)s;
    zsm->fd = fd;
    zsm->sm = sm;
    return s;
}

void ashmem_sem_destroy(sem_t* s) {
    ashmem_sem_t* zsm = (ashmem_sem_t*)s;
    int fd = zsm->fd;
    void* sm = zsm->sm;
    int r = sem_destroy(s);
    assertion(r == 0, "sem_destroy=%d", r);
    r = munmap(sm, PAGE_SIZE);
    assertion(r == 0, "munmap(0x%08X)=%d", sm, r);
    r = close(fd);
    assertion(r == 0, "close(fd=%d)=%d", fd, r);
}

/* see: http://www.akkadia.org/drepper/futex.pdf */

void futex_lock(int volatile* g) {
    int c = __atomic_cmpxchg(0, 1, g);
    if (c != 0) {
        if (c != 2) {
            c = __atomic_swap(2, g);
        }
        while (c != 0) {
            __futex_wait(g, 2, null);
            c = __atomic_swap(2, g);
        }
    }
}

void futex_unlock(int volatile* g) {
    int c = __atomic_dec(g);
    if (c != 1) {
        *g = 0;
        __futex_wake(g, 1);
    }
}

static volatile int spins_lock_nowait;
static volatile int spins_count;
static volatile int spins_futex_wait;
static volatile int spins_unlock_nowake;
static volatile int spins_unlock_wake;

static void futex_lock_count_spins(int volatile* g) {
    int c = __atomic_cmpxchg(0, 1, g);
    if (c != 0) {
        if (c != 2) {
            c = __atomic_swap(2, g);
        }
        while (c != 0) {
            __atomic_inc(&spins_futex_wait);
            __futex_wait(g, 2, null);
            c = __atomic_swap(2, g);
        }
        __atomic_inc(&spins_count);
    } else {
        __atomic_inc(&spins_lock_nowait);
    }
}

static void futex_unlock_count_spins(int volatile* g) {
    int c = __atomic_dec(g);
    if (c != 1) {
        *g = 0;
        __atomic_inc(&spins_unlock_wake);
        __futex_wake(g, 1);
    } else {
        __atomic_inc(&spins_unlock_nowake);
    }
}

typedef struct {
    int tn;
    int N;
    void (*foo)(int tn, int i);
    char* label;
} thread_params;

static void* thread(void* vp) {
    thread_params p = *(thread_params*)vp;
    if (get_cpu_count() > 0) {
        unsigned int mask = get_thread_affinity();
        unsigned int m = nth_set_bit(mask, p.tn);
        if (m != 0) {
            bool b = set_thread_affinity(m);
            mask = get_thread_affinity();
            assertion(b && mask == m, "%s set_thread_affinity failed with result %d and mask=0x%08X (expected 0x%08X)", p.label, b, mask, m);
        }
    }
    char s[256];
    sprintf(s, "%s thread%d", p.label, p.tn);
/*  timestamp(s); */
    for (int i = 0; i < p.N; i++) {
        p.foo(p.tn, i);
    }
/*  int64 t = timestamp(s);
    trace("%s %lld nanoseconds per call", p.label, t / p.N); */
    return null;
}

/* because of possible priority inversion at least 3 threads
   test of if synchronization primitives is recommended */

static void smoke_test(thread_params p, int nt) {
    pthread_t threads[nt];
    thread_params tp[nt];
    for (int i = 0; i < nt; i++) {
        tp[i] = p;
        tp[i].tn = i + 1;
        int r = pthread_create(&threads[i], null, thread, &tp[i]);
        assertion(r == 0, "pthread_create failed=%d", r);
    }
    for (int i = 0; i < nt; i++) {
        void* rv = null;
        int r = pthread_join(threads[i], &rv);
        assertion(r == 0, "pthread_join failed=%d", r);
    }
}

static volatile int locked;
static volatile int resource;

static void futex_spins_test(int tn, int i) {
    futex_lock_count_spins(&locked);
    resource = i;
    int local = resource;
    assertion(local == i, "resource=%d expected %d", local, i);
    resource = 0;
    futex_unlock_count_spins(&locked);
}

static void futex_test(int tn, int i) {
    futex_lock(&locked);
    resource = i;
    int local = resource;
    assertion(local == i, "resource=%d expected %d", local, i);
    resource = 0;
    futex_unlock(&locked);
}

void futex_smoke_test() {
    spins_lock_nowait = 0;
    spins_count = 0;
    spins_futex_wait = 0;
    spins_unlock_nowake = 0;
    spins_unlock_wake = 0;
    const int N = 100000;
    thread_params p = {0, N, futex_spins_test, "futex_spins"};
    smoke_test(p, 3);
/*  trace("spins of %d lock_nowait=%d count=%d futex_wait=%d unlock_nowake=%d unlock_wake=%d",
          N * 3, spins_lock_nowait, spins_count, spins_futex_wait, spins_unlock_nowake, spins_unlock_wake); */
    /* spins on 300,000 lock_nowait=290936 count=9064 futex_wait=4908 unlock_nowake=289375 unlock_wake=10625 */
    p.foo = futex_test;
    p.label = "futex";
    smoke_test(p, 3);
}

static sem_t* semaphore;

static void sem_test(int tn, int i) {
    int r = sem_wait(semaphore);
    assertion(r == 0, "sem_wait(0x%08X) failed with result %d errno=%d", semaphore, r, errno);
    r = sem_post(semaphore);
    assertion(r == 0, "sem_post(0x%08X) failed with result %d errno=%d", semaphore, r, errno);
}

void sem_smoke_test() {
    semaphore = ashmem_sem_create("semaphore");
    assertion(semaphore != null, "sem_init failed semaphore=0x%08X errno=%d", semaphore, errno);
    if (semaphore != null) {
        int r = sem_post(semaphore);
        assertion(r == 0, "sem_post(0x%08X) failed with result %d errno=%d", semaphore, r, errno);
        const int N = 100000;
        thread_params p = {0, N, sem_test, "sem_test"};
        smoke_test(p, 3);
        ashmem_sem_destroy(semaphore);
        semaphore = null;
    }
}

static pthread_mutex_t* mutex;

static void mutex_test(int tn, int i) {
    int r = pthread_mutex_lock(mutex);
    assertion(r == 0, "pthread_mutex_lock(0x%08X) failed with result %d errno=%d", mutex, r, errno);
    r = pthread_mutex_unlock(mutex);
    assertion(r == 0, "pthread_mutex_unlock(0x%08X) failed with result %d errno=%d", mutex, r, errno);
}

static void mapsl_test(int tn, int i) {
    mapsl_smoke_test(false);
}

static void mapll_test(int tn, int i) {
    mapll_smoke_test(false);
}

static void sync_uncontested() {
    sem_t semaphore;
    sem_init(&semaphore, false, 1);
    const int N = 1000;
    timestamp("semaphore uncontested");
    for (int i = 0; i < N; i++) {
        int r = sem_wait(&semaphore);
        assertion(r == 0, "sem_wait(0x%08X) failed with result %d errno=%d", semaphore, r, errno);
        r = sem_post(&semaphore);
        assertion(r == 0, "sem_post(0x%08X) failed with result %d errno=%d", semaphore, r, errno);
    }
    timestamp("semaphore uncontested");
    sem_destroy(&semaphore);

    pthread_mutex_t mutex;
    pthread_mutex_init(&mutex, null);
    timestamp("mutex uncontested");
    for (int i = 0; i < N; i++) {
        int r = pthread_mutex_lock(&mutex);
        assertion(r == 0, "pthread_mutex_lock(0x%08X) failed with result %d errno=%d", mutex, r, errno);
        r = pthread_mutex_unlock(&mutex);
        assertion(r == 0, "pthread_mutex_unlock(0x%08X) failed with result %d errno=%d", mutex, r, errno);
    }
    pthread_mutex_destroy(&mutex);
    timestamp("mutex uncontested");

    int futex = 0;
    timestamp("futex uncontested");
    for (int i = 0; i < N; i++) {
        futex_lock(&futex);
        futex_unlock(&futex);
    }
    timestamp("futex uncontested");

    spins_lock_nowait = 0;
    spins_count = 0;
    spins_futex_wait = 0;
    spins_unlock_nowake = 0;
    spins_unlock_wake = 0;
    futex = 0;
    for (int i = 0; i < N; i++) {
        futex_lock_count_spins(&futex);
        futex_unlock_count_spins(&futex);
    }
    assert(spins_lock_nowait == N);
    assert(spins_unlock_nowake == N);
    assert(spins_unlock_wake == 0);
    assert(spins_futex_wait == 0);
/*
    trace("spins of %d lock_nowait=%d count=%d futex_wait=%d unlock_nowake=%d unlock_wake=%d",
          N, spins_lock_nowait, spins_count, spins_futex_wait, spins_unlock_nowake, spins_unlock_wake);
    // sync_uncontested spins of 1000 lock_nowait=1000 count=0 futex_wait=0 unlock_nowake=1000 unlock_nowake=0
*/
}


void mutex_smoke_test() {
    mutex = ashmem_mutex_create("mutex");
    assertion(mutex != null, "ashmem_mutex_create failed mutex=0x%08X errno=%d", mutex, errno);
    if (mutex != null) {
        const int N = 100000;
        thread_params p = {0, N, mutex_test, "mutex_test"};
        smoke_test(p, 3);
        ashmem_mutex_destroy(mutex);
        mutex = null;
    }
    {
        const int N = 2;
        thread_params p = {0, N, mapsl_test, "mapsl_smoke_test"};
        smoke_test(p, 3);
    }
    {
        const int N = 2;
        thread_params p = {0, N, mapll_test, "mapll_smoke_test"};
        smoke_test(p, 3);
    }
    sync_uncontested();
}
